from wlkata_mirobot import WlkataMirobot
from PyQt5.Qt import *
from PyQt5.Qt import QThread
import time
import logging

robot_logger=logging.getLogger("Robot-Driver")

#### 示教点位
P_QR1 = [-23.6, 68.696, -25.078, -5.063, -41.431, 8.159, 242.71, -107.789, 39.937, -3.303, 2.791, -12.098, 0.0]
P_QR2 = [-10.599, 62.299, -15.0, -4.797, -39.204, 8.5, 246.661, -47.757, 42.591, -2.032, 8.577, 1.045, 0.0]
P_QR3 = [5.597, 62.599, -15.094, -4.406, -39.499, 8.591, 250.071, 23.06, 41.61, -1.757, 8.438, 17.099, 0.0]
P_QR4 = [19.744, 65.95, -20.453, -4.297, -42.499, 8.954, 244.971, 86.497, 40.301, -2.499, 3.506, 31.608, 0.0]

#### arm为声明的机器人对象
arm = WlkataMirobot()

class Robot_Driver():

    arm.home()

    #### 机器人复位
    def robot_Home(self):
        arm.home()
    
    #### 机器人动作
    def robot_Action(self, value, cmd):
        if value != []:
            match cmd:
                case 19:
                    arm.home()
                    time.sleep(1)
    
                #### QR2
                case 22:
                    #### 抓取点,设置工具坐标，机器人通过世界坐标进行点位移动，默认点控模式为：p2p
                    arm.set_tool_pose(x=198.67,y=0.0,z=230.72,roll=0.0,pitch=0.0,yaw=0.0)

                    ### 放置操作 设置轴角坐标，机器人通过轴角坐标进行点位移动，更加平滑的到达指定点位
                    arm.set_joint_angle(P = P_QR2)                  
        
        else:
            arm.home()    

    #### 获取机器人当前姿态 （Home. Busy, Idle, Alarm）
    def get_Robot_Status(self,):
        try:
            robot_state = arm.status.state   
            return robot_state

        except Exception as e:
            robot_logger.error("can not get robot {}'s status! Reason:{}".format(e))
            return None
    
    #### 获取机械臂当前姿态（5，6，7，1，2，3，X,Y,Z,A,B,C)
    def get_Gesture(self,):
        try:    
            gesture = arm.status()
            return gesture
        except Exception as e:
            robot_logger.error("can not get robot {}'s current getsture,reason:{}".format,e)
            return None

    #### 设置速度百分比 (默认：2000)
    def set_Speed_per(self, speed):
        try:
            arm.set_speed(speed)
        except Exception as e:
            robot_logger.error("couldn't set robot {}'s Speed percentage , reason : {}".format,e)
